/*****************************************************************
**          大连理工大学 凌BUG战队&大连民族大学 C.ONE战队
**                **生死有命，富贵在天**
**    没有什么难题是一个通宵解决不了的，如果有那就整两个！
**---------------------------------------------------------------
** 项目名称：   dlmu-hero-robot
** 日    期：   2024-03-12
** 作    者：   MasterWang & Changed by TuxMonkey
**---------------------------------------------------------------
** 文 件 名：   motor_control.c
** 文件说明：   机器人电机、超级电容控制和运动解算
*****************************************************************/

/*---------------------INCLUDES----------------------*/
#include "motor_control.h"
#include "referee_system.h"

#include "can.h"
#include "tim.h"
#include "usart.h"
#include "bmi088.h"
#include "IMU.h"
#include "PID.h"

/*---------------------VARIABLES---------------------*/
// 使用CCM数据RAM区(0x10004000-0x10004FFF 4KB)
//
Motor_Status_t motor_status_chassis[4] __attribute__((at(0x10004000))) = {0};
Motor_Status_t motor_status_gimbal[2] __attribute__((at(0x10004200))) = {0};
Motor_Status_t motor_status_ammobooster[3] __attribute__((at(0x10004400))) = {0};
Motor_Status_Ex_t motor_status_ex_turnbullet __attribute__((at(0x10004600))) = {0};

float gimbal_angle_deg[2] = {0.0f};
float32_t angle = 0.0f;
float32_t angleReverse = 0.0f;
//
Robot_Motor_Speed_t robot_motor_speed __attribute__((at(0x10004800))) = {0};

//
float supercap_voltage __attribute__((at(0x10004A00))) = 0.0f; // 电压值
float supercap_energy_percent __attribute__((at(0x10004A10))) = 0.0f;

//
uint8_t chassis_reset_id_flag __attribute__((at(0x10004A20))) = 0; // 进入底盘电机快速ID设置程序的标志位

/*---------------------FUNCTIONS---------------------*/

/***********************************************************************
** 函 数 名： UpdateMotorStatus()
** 函数说明： 解析并更新机器人电机基本状态(CAN接收中断调用)
**---------------------------------------------------------------------
** 输入参数： 电机状态变量指针,CAN接收原始数据
** 返回参数： 无
***********************************************************************/
void UpdateMotorStatus(Motor_Status_t *ptr, uint8_t *data)
{
	//
	ptr->angle = (uint16_t)((data[0] << 8) | data[1]);
	ptr->speed_rpm = (int16_t)((data[2] << 8) | data[3]);
	ptr->current = (int16_t)((data[4] << 8) | data[5]);
	ptr->temperature = data[6];
}

/***********************************************************************
** 函 数 名： UpdateMotorStatusEx()
** 函数说明： 更新机器人电机扩展状态(CAN接收中断调用)
**---------------------------------------------------------------------
** 输入参数： 电机扩展状态变量指针,电机基本状态变量指针
** 返回参数： 无
***********************************************************************/
void UpdateMotorStatusEx(Motor_Status_Ex_t *ptr_ex, Motor_Status_t *ptr)
{
	//
	ptr_ex->delt_angle = ptr->angle - ptr_ex->last_angle; // 呈现了一个电机的位置环，圈数cnt 角度angle，利用

	if (ptr_ex->delt_angle < -4096)
	{
		ptr_ex->round_cnt++;
	}
	else if (ptr_ex->delt_angle > 4096)
	{
		ptr_ex->round_cnt--;
	}

	ptr_ex->total_angle = ptr_ex->round_cnt * 8192 + ptr->angle - ptr_ex->offset_angle;

	ptr_ex->last_angle = ptr->angle;
}

/***********************************************************************
** 函 数 名： HAL_CAN_RxFifo0MsgPendingCallback()
** 函数说明： CAN_FIFO0接收中断回调函数
**---------------------------------------------------------------------
** 输入参数： CAN句柄
** 返回参数： 无
***********************************************************************/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8];
	uint8_t i;

	HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

	if (hcan == &hcan1)
	{
		switch (rx_header.StdId)
		{
		case CAN_3508_MOTOR1_ID:
		case CAN_3508_MOTOR2_ID:
		case CAN_3508_MOTOR3_ID:
		case CAN_3508_MOTOR4_ID:
		{
			i = rx_header.StdId - CAN_3508_MOTOR1_ID;
			UpdateMotorStatus(&motor_status_chassis[i], rx_data);
			break;
		}

		default:
			break;
		}
	}
	else if (hcan == &hcan2)
	{
		switch (rx_header.StdId)
		{
		case CAN_3508_SHOOT1_ID:
		{
			UpdateMotorStatus(&motor_status_ammobooster[0], rx_data);
			break;
		}
		case CAN_6020_PIT_ID:
		{
			UpdateMotorStatus(&motor_status_gimbal[1], rx_data);
			motor_status_gimbal[1].angle = (motor_status_gimbal[1].angle + 4324) % 8192; // 软件偏移6020绝对角度
			gimbal_angle_deg[1] = -1 * (motor_status_gimbal[1].angle - PIT6020_HORIZONTAL_POSITION) * 360.0f / 8192;
			break;
		}
		case CAN_3508_SHOOT2_ID:
		{
			UpdateMotorStatus(&motor_status_ammobooster[1], rx_data);
			break;
		}

		default:
			break;
		}
	}
}

/***********************************************************************
** 函 数 名： HAL_CAN_RxFifo1MsgPendingCallback()
** 函数说明： CAN_FIFO1接收中断回调函数
**---------------------------------------------------------------------
** 输入参数： CAN句柄
** 返回参数： 无
***********************************************************************/
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) //
{
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8];
	uint8_t *ptr;

	HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data);

	if (hcan == &hcan1)
	{
		switch (rx_header.StdId)
		{
		case CAN_6020_YAW_ID:
		{
			UpdateMotorStatus(&motor_status_gimbal[0], rx_data);
			motor_status_gimbal[0].angle = (motor_status_gimbal[0].angle + 750) % 8192; // 软件偏移6020绝对角度 111 修改至此以改变云台朝向
			gimbal_angle_deg[0] = (motor_status_gimbal[0].angle - YAW6020_45DEGREE_POSITION) * 360.0f / 8192;
			break;
		}

		case CAN_3508_TURNBULLET_ID:
		{
			UpdateMotorStatus(&motor_status_ammobooster[2], rx_data);
			UpdateMotorStatusEx(&motor_status_ex_turnbullet, &motor_status_ammobooster[2]);
			break;
		}

		case CAN_SUPERCAP_STAT1_ID:
		{
			//CAN ID 为 0x301
			// ptr=(uint8_t *)&supercap_voltage; ptr[0]=rx_data[0]; ptr[1]=rx_data[1]; ptr[2]=rx_data[2]; ptr[3]=rx_data[3];
			// ptr=(uint8_t *)&supercap_energy_percent; ptr[0]=rx_data[4]; ptr[1]=rx_data[5]; ptr[2]=rx_data[6]; ptr[3]=rx_data[7];
			// break;
			// 解析新超级电容的CAN信息
			uint8_t supercap_state = rx_data[0]; // 电容状态（1字节）
			// rx_data[1]为保留位，忽略
			uint16_t cap_voltage_raw = (rx_data[2] << 8) | rx_data[3]; // 电压原始值（小端，单位毫伏）
			float chassis_power;									   // 底盘功率（float）
			// 更新电压（转换为伏特）
			supercap_voltage = (float)cap_voltage_raw / 1000.0f;

			// 解析底盘功率（float类型，小端字节序）
			uint8_t *ptr = (uint8_t *)&chassis_power;
			ptr[0] = rx_data[4];
			ptr[1] = rx_data[5];
			ptr[2] = rx_data[6];
			ptr[3] = rx_data[7];
			supercap_energy_percent = chassis_power; // 根据需求调整变量名或新增变量

			break;
		}
		default:
			break;
		}
	}
}

/***********************************************************************
** 函 数 名： CAN_cmd_chassis_reset_ID()
** 函数说明： 底盘3508电机进入快速设置ID功能(短按SET再长按SET)
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void CAN_cmd_chassis_reset_ID(void)
{
	CAN_TxHeaderTypeDef tx_header;
	uint8_t tx_data[8];
	uint32_t tx_mail_box;

	tx_header.StdId = 0x700;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 0x08;

	tx_data[0] = 0;
	tx_data[1] = 0;
	tx_data[2] = 0;
	tx_data[3] = 0;
	tx_data[4] = 0;
	tx_data[5] = 0;
	tx_data[6] = 0;
	tx_data[7] = 0;

	HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data, &tx_mail_box);
}

/***********************************************************************
** 函 数 名： CAN_cmd_motor()
** 函数说明： CAN发送电机控制指令基础函数
**---------------------------------------------------------------------
** 输入参数： can句柄,id,数据1,数据2,数据3,数据4
** 返回参数： 无
***********************************************************************/
void CAN_cmd_motor(CAN_HandleTypeDef *hcan, uint32_t identifier, int16_t data1, int16_t data2, int16_t data3, int16_t data4)
{
	CAN_TxHeaderTypeDef tx_header;
	uint8_t tx_data[8];
	uint32_t tx_mail_box;

	tx_header.StdId = identifier;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 0x08;

	tx_data[0] = (data1 >> 8);
	tx_data[1] = data1;
	tx_data[2] = (data2 >> 8);
	tx_data[3] = data2;
	tx_data[4] = (data3 >> 8);
	tx_data[5] = data3;
	tx_data[6] = (data4 >> 8);
	tx_data[7] = data4;

	HAL_CAN_AddTxMessage(hcan, &tx_header, tx_data, &tx_mail_box);
}

/***********************************************************************
** 函 数 名： CAN_cmd_chassis()
** 函数说明： 发送底盘电机控制指令
**---------------------------------------------------------------------
** 输入参数： 底盘电机1控制值,底盘电机2控制值,底盘电机3控制值,底盘电机4控制值
** 返回参数： 无
***********************************************************************/
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
	// 底盘电机->can1
	CAN_cmd_motor(&hcan1, CAN_MOTOR_CTRL_ID_1_4, motor1, motor2, motor3, motor4);
}

/***********************************************************************
** 函 数 名： CAN_cmd_gimbal_ammobooster()
** 函数说明： 发送云台和发射机构电机控制指令
**---------------------------------------------------------------------
** 输入参数： 云台yaw电机控制值,云台pitch电机控制值,摩擦轮电机1控制值,摩擦轮电机2控制值,拨弹电机控制值
** 返回参数： 无
***********************************************************************/
void CAN_cmd_gimbal_ammobooster(int16_t yaw, int16_t pitch, int16_t shoot1, int16_t shoot2, int16_t turnbullet)
// void CAN_cmd_gimbal_ammobooster(int16_t yaw, int16_t pitch, int16_t shoot1, int16_t shoot2, int16_t turnbullet)
{
	//	//yaw电机->can1, pitch电机->can2
	//	//摩擦轮电机->can2, 拨弹电机->can2
	//	CAN_cmd_motor(&hcan1, CAN_MOTOR_CTRL_ID_5_8, yaw, 0, 0, 0);
	//	CAN_cmd_motor(&hcan2, CAN_MOTOR_CTRL_ID_5_8, shoot1, pitch, turnbullet, shoot2);
	// 拨弹电机->can1, yaw电机->can1
	// pitch电机->can2,摩擦轮电机->can2
	//	CAN_cmd_motor(&hcan1, CAN_MOTOR_CTRL_ID_5_8, turnbullet,0, 0 ,yaw);
	CAN_cmd_motor(&hcan1, CAN_MOTOR_CTRL_ID_5_8, yaw, turnbullet, 0, 0);
	CAN_cmd_motor(&hcan2, CAN_MOTOR_CTRL_ID_5_8, pitch, shoot1, shoot2, 0);

	//	CAN_cmd_motor(&hcan1, CAN_MOTOR_CTRL_ID_5_8, turnbullet,0, 0 ,7500 );
	//	CAN_cmd_motor(&hcan2, CAN_MOTOR_CTRL_ID_5_8, 6600, shoot1, shoot2, 0);
}

/***********************************************************************
** 函 数 名： set_chassis_motor_speed()
** 函数说明： 控制底盘电机转速(在500Hz定时器中断调用)
**---------------------------------------------------------------------
** 输入参数： 底盘四个轮子电机速度设定值
** 返回参数： 无
***********************************************************************/
void set_chassis_motor_speed(float wheel_speed[])
{
	uint8_t i;
	int16_t output_chassis[4];

	for (i = 0; i < 4; i++)
	{
		pid_chassis[i].Ref = wheel_speed[i];
		output_chassis[i] = PIDIncControl(&pid_chassis[i], motor_status_chassis[i].speed_rpm);
	}

	//
	CAN_cmd_chassis(output_chassis[0], output_chassis[1], output_chassis[2], output_chassis[3]);
}

/***********************************************************************
** 函 数 名： set_gimbal_ammobooster_motor_speed()
** 函数说明： 控制云台及发射机构电机转速(在500Hz定时器中断调用)
**---------------------------------------------------------------------
** 输入参数： 云台yaw轴设定速度,云台pitch轴设定速度,摩擦轮设定速度,拨弹电机设定转速（云台控速使用bmi088陀螺仪的角速度作为反馈值）
** 返回参数： 无
***********************************************************************/
void set_gimbal_ammobooster_motor_speed(int16_t yaw_speed, int16_t pitch_speed, int16_t shoot_speed, int16_t turnbullet_speed)
{
	float yaw_axis_speed, pitch_axis_speed;
	int16_t output_yaw, output_pitch, output_shoot[2], output_turnbullet;

	//
	pid_gimbal[0].Ref = yaw_speed;

	arm_sqrt_f32(bmi088_gyro_deg_calib[BMI088_GYRO_YAW_INDEX] * bmi088_gyro_deg_calib[BMI088_GYRO_YAW_INDEX] + bmi088_gyro_deg_calib[BMI088_GYRO_ROLL_INDEX] * bmi088_gyro_deg_calib[BMI088_GYRO_ROLL_INDEX], &yaw_axis_speed);
	if (bmi088_gyro_deg_calib[BMI088_GYRO_YAW_INDEX] < 0)
	{
		yaw_axis_speed = -yaw_axis_speed;
	}
	output_yaw = -PIDIncControl(&pid_gimbal[0], yaw_axis_speed); // 方向反了,修改这里正负号

	//
	pid_gimbal[1].Ref = pitch_speed;
	pitch_axis_speed = bmi088_gyro_deg_calib[BMI088_GYRO_PITCH_INDEX];
	output_pitch = -1 * PIDIncControl(&pid_gimbal[1], pitch_axis_speed); // 方向反了,修改这里正负号

	// 3508摩擦轮
	pid_ammobooster[0].Ref = -shoot_speed*0.73; // 方向反了,修改这里正负号
	output_shoot[0] = PIDIncControl(&pid_ammobooster[0], motor_status_ammobooster[0].speed_rpm);

	pid_ammobooster[1].Ref = shoot_speed*0.73; // 方向反了,修改这里正负号
	output_shoot[1] = PIDIncControl(&pid_ammobooster[1], motor_status_ammobooster[1].speed_rpm);

	// snail摩擦轮
	//__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, shoot_speed);
	//__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, shoot_speed);

	//
	pid_ammobooster[2].Ref = -turnbullet_speed; // 方向反了,修改这里正负号
	output_turnbullet = PIDIncControl(&pid_ammobooster[2], motor_status_ammobooster[2].speed_rpm);

	//
	CAN_cmd_gimbal_ammobooster(output_yaw, output_pitch, output_shoot[0], output_shoot[1], output_turnbullet); // 单独调试速度时修改这里,不想控制的电机给0
}

/***********************************************************************
** 函 数 名： set_gimbal_imu_angle()
** 函数说明： 使用IMU反馈角度控制云台角度位置(在机器人运动解算中调用)
**---------------------------------------------------------------------
** 输入参数： 云台yaw轴设定角度,云台pitch轴设定角度
** 返回参数： 无
***********************************************************************/
void set_gimbal_imu_angle(float yaw_angle, float pitch_angle)
{
	//
	pid_yaw.Ref = yaw_angle;
	robot_motor_speed.yaw_speed = PIDPosControl(&pid_yaw, IMU_yaw_angle_dial);

	//
	pid_pitch.Ref = -pitch_angle;
	robot_motor_speed.pitch_speed = PIDPosControl(&pid_pitch, IMU_angle_deg[IMU_PITCH_INDEX]);
}

/***********************************************************************
** 函 数 名： set_gimbal_6020_angle()
** 函数说明： 使用6020反馈角度控制云台角度位置，现专用于能量机关击打，需要底盘静止、45度为yaw零点、且限定yaw范围±45度(在机器人运动解算中调用)
**---------------------------------------------------------------------
** 输入参数： 云台yaw轴设定角度,云台pitch轴设定角度
** 返回参数： 无
***********************************************************************/
void set_gimbal_6020_angle(float yaw_angle, float pitch_angle)
{
	//
	pid_yaw.Ref = yaw_angle;
	robot_motor_speed.yaw_speed = PIDPosControl(&pid_yaw, gimbal_angle_deg[0]);

	//
	pid_pitch.Ref = pitch_angle;
	// robot_motor_speed.pitch_speed = PIDPosControl(&pid_pitch, gimbal_angle_deg[1]);
	robot_motor_speed.pitch_speed = PIDPosControl(&pid_pitch, IMU_angle_deg[IMU_PITCH_INDEX]);
}

/***********************************************************************
** 函 数 名： robot_motion_resolving()
** 函数说明： 机器人运动解算,控制指令->电机速度解算(在200Hz定时器中断调用)
**---------------------------------------------------------------------
** 输入参数： 机器人控制指令变量地址, 机器人电机速度变量地址
** 返回参数： 无
***********************************************************************/
void robot_motion_resolving(ROBOT_ctrl_t *ctrl, Robot_Motor_Speed_t *motor_speed)
{
	uint8_t i;

	float tmp;
	float max_wheelspeed = 0.0f;
	float wheelspeed_scale = 1.0f;
	static float chassis_motor_maxspeed = 5000.0f;

	float vw_set = 0.0f;					  // rpm//与陀螺仪角度的差角即陀螺仪的相对角度与初始的角度作为绝对角度的差值
	float vx_resolve, vy_resolve, vw_resolve; // mm/s

	/******************底盘运动分解***************/
	// vw_set解算
	if (ctrl->movement_mode == CHASSIS_FOLLOW_GIMBAL) // 底盘跟随云台
	{
		if (ctrl->chassis_lock_flag == 1) // 底盘旋转锁定
		{
			vw_set = 0.0f;
		}
		else
		{
			pid_rotation.Ref = YAW6020_FORWARD_POSITION * 360.0f / 8192;						 // 正面对敌
			vw_set = PIDPosControl(&pid_rotation, motor_status_gimbal[0].angle * 360.0f / 8192); // 方向反了,修改这里正负号
		}
	}
	else if (ctrl->movement_mode == CHASSIS_GYRO_ROTATION) // 底盘小陀螺旋转
	{
		if (ctrl->chassis_accelerate_flag == 1)
		{
			vw_set = 45.0f;
		}
		else
		{
			vw_set = 30.0f;
		}
	}

	// 底盘四个麦轮电机速度解算 mm/s->rpm
	// float32_t angle = 0;

	if (ctrl->movement_mode == CHASSIS_FOLLOW_GIMBAL) // 底盘跟随云台
	{
		angle = (YAW6020_FORWARD_POSITION - motor_status_gimbal[0].angle) * 6.2831853f / 8192; // 简单测试时angle=0

		vx_resolve = ctrl->vx_set * arm_cos_f32(angle) + ctrl->vy_set * arm_sin_f32(angle); // 这个根据麦克纳姆轮在运动中的力学分析得来的在麦轮上，每个小纶旋转都有一个方向的分力，旋转方向不一样，分力方向也不同
		vy_resolve = ctrl->vy_set * arm_cos_f32(angle) - ctrl->vx_set * arm_sin_f32(angle); // 角度为云台yaw轴角度当前值，与最开始确定机器人正方向时的云台角，保证确定一个方向为正方向
		vw_resolve = vw_set * 65.89869f;													// sqrt(2)*v*cos(45-arctan(378/418))=(w/60)*2*PI*sqrt(424*424+465*465)
	}
	else if (ctrl->movement_mode == CHASSIS_GYRO_ROTATION) // 底盘小陀螺旋转
	{
		angleReverse = (motor_status_gimbal[0].angle - YAW6020_FORWARD_POSITION) * 6.2831853f / 8192; // 2pi=6.2831853f

		vx_resolve = ctrl->vx_set * arm_cos_f32(angleReverse) + ctrl->vy_set * arm_sin_f32(angleReverse); // 这个根据麦克纳姆轮在运动中的力学分析得来的在麦轮上，每个小纶旋转都有一个方向的分力，旋转方向不一样，分力方向也不同
		vy_resolve = ctrl->vy_set * arm_cos_f32(angleReverse) - ctrl->vx_set * arm_sin_f32(angleReverse); // 角度为云台yaw轴角度当前值，与最开始确定机器人正方向时的云台角，保证确定一个方向为正方向
		vw_resolve = vw_set * 65.89869f;																  // sqrt(2)*v*cos(45-arctan(378/418))=(w/60)*2*PI*sqrt(424*424+465*465)
	}

	motor_speed->chassis_speed[0] = (-(vx_resolve + vy_resolve + vw_resolve)) * 2.40494601831f; // v_set/(pi*152.5) * 60 * (3591/187)
	motor_speed->chassis_speed[1] = (vx_resolve - vy_resolve - vw_resolve) * 2.40494601831f;	//
	motor_speed->chassis_speed[2] = (vx_resolve + vy_resolve - vw_resolve) * 2.40494601831f;
	motor_speed->chassis_speed[3] = (-(vx_resolve - vy_resolve + vw_resolve)) * 2.40494601831f;

	// 超级电容低压保护
	if (supercap_energy_percent <= 20.0f)
	{
		chassis_motor_maxspeed = chassis_motor_maxspeed * 0.9f + (4000 + (supercap_energy_percent - 20.0f) * 200) * 0.1f;
	} // 防止电容功率消耗过大导致电容消耗过大，使电压过低而电压降低会一定程度上在电机上产生反向转矩使电机速度开始下降。
	else
	{
		chassis_motor_maxspeed = 4000.0f;
	}

	// 小陀螺和加速移动时,无视电容低压保护
	if ((ctrl->movement_mode == CHASSIS_GYRO_ROTATION) || (ctrl->chassis_accelerate_flag == 1))
	{
		chassis_motor_maxspeed = 8000.0f;
	}

	// 限制轮子的最大速度
	for (i = 0; i < 4; i++)
	{
		tmp = ABS(motor_speed->chassis_speed[i]); // 防止转的太快，功率消耗过大，导致暴毙
		if (tmp > max_wheelspeed)
		{
			max_wheelspeed = tmp;
		}
	}
	//	if(max_wheelspeed > chassis_motor_maxspeed)
	//	{
	//		wheelspeed_scale = chassis_motor_maxspeed / max_wheelspeed;//这里便是限制功率
	//		for(i=0;i<4;i++) {motor_speed->chassis_speed[i] *= wheelspeed_scale;}
	//	}
	//
	/******************云台角度控制*************************/
	//
	if (ctrl->energy_aiming_flag == 1) // 能量机关模式，打能量机关能开挂谁手打啊，不好打
	{
		set_gimbal_6020_angle(ctrl->yaw_angle_set_energy, ctrl->pitch_angle_set); // 打能量机关这里是要开自瞄的，云台的一个跟踪，也就是妙算对图像处理对物体运动的预判。
	}
	else // 普通模式
	{
		set_gimbal_imu_angle(ctrl->yaw_angle_set, ctrl->pitch_angle_set);
		//		set_gimbal_imu_angle(20, 10);
	}

	//
	uint8_t tx_buf[5] = {0};
	int16_t tmp_pitch = (IMU_angle_deg[IMU_PITCH_INDEX]) * 100;
	tx_buf[0] = 0x55;
	tx_buf[1] = 0x11;
	tx_buf[2] = (tmp_pitch >> 8);
	tx_buf[3] = tmp_pitch;
	tx_buf[4] = (tx_buf[1] + tx_buf[2] + tx_buf[3]);
	SendUART6DataPackage(tx_buf, 5);

	/******************摩擦轮和红点激光器控制***************/
	//
	if (ctrl->shoot_open_flag == 1)
	{
		HAL_GPIO_WritePin(LASER_GPIO_Port, LASER_Pin, GPIO_PIN_SET); // open laser

		// 3508摩擦轮 0-8000
		if (robot_referee_status.game_robot_status.shooter_id1_42mm_speed_limit == 15)//15
		{
			motor_speed->shoot_speed = 8000;//8000 
		}
		else if (robot_referee_status.game_robot_status.shooter_id1_42mm_speed_limit == 18)//18
		{
			motor_speed->shoot_speed = 8000;
		}
		else if (robot_referee_status.game_robot_status.shooter_id1_42mm_speed_limit == 30)//30
		{
			motor_speed->shoot_speed = 8000;
		}
		else
		{
			motor_speed->shoot_speed = 8000;
		}

		// snail摩擦轮 400-2200
		// if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==15) {motor_speed->shoot_speed = 1260;}
		// else if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==18) {motor_speed->shoot_speed = 1300;}
		// else if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==30) {motor_speed->shoot_speed = 1600;}
		// else {motor_speed->shoot_speed = 1260;}
	}
	else
	{
		HAL_GPIO_WritePin(LASER_GPIO_Port, LASER_Pin, GPIO_PIN_RESET); // close laser//关闭时同时关闭摩擦轮

		motor_speed->shoot_speed = 0; // 3508摩擦轮
		// motor_speed->shoot_speed = 400; //snail摩擦轮
	}

	/******************拨弹动作控制***************/
	//
	if ((ctrl->turnbullet_flag == 1) || (ctrl->turnbullet_once_flag == 1))
	{
		// 判断枪口热量超限(按住Z无视)
		if ((robot_referee_status.power_heat_data.shooter_id1_42mm_cooling_heat >= (robot_referee_status.game_robot_status.shooter_id1_42mm_cooling_limit * 0.7)) && (ctrl->ignore_shoot_heat_limit_flag == 0))
		{
			motor_speed->turnbullet_speed = 100;
		}
		else
		{
			uint16_t turnbullet_freq;

			// 根据冷却速率调整射频 步兵40 60 80
			if (ctrl->turnbullet_once_flag == 1)
			{
				turnbullet_freq = 5;
			}
			else
			{
				if (ctrl->auto_aiming_flag == 1)
				{
					turnbullet_freq = 3 * (robot_referee_status.game_robot_status.shooter_id1_42mm_cooling_rate / 10);
					if (turnbullet_freq > 20)//20
					{
						turnbullet_freq = 20;//20
					} // 最大射频20Hz
				}
				else
				{
					turnbullet_freq = 2 * (robot_referee_status.game_robot_status.shooter_id1_42mm_cooling_rate / 10); // 射速这玩意在实际测量时有所变化
					if (turnbullet_freq > 16)//16
					{
						turnbullet_freq = 16;
					} // 最大射频16Hz
				}
			}

			motor_speed->turnbullet_speed = turnbullet_freq *45.57143f; // Hz->rpm (shoot_freq*60/7)*36
		}
	}
	else if (ctrl->turnbullet_reverse_flag == 1)
	{
		motor_speed->turnbullet_speed = -600.0f; //~2Hz
	}
	else
	{
		motor_speed->turnbullet_speed = 0;
	}

	/******************弹舱盖动作控制***************/
	//
	if (ctrl->bulletroom_open_flag == 1)
	{
		__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, BULLETROOM_COVER_OPEN); // open//这里一般是舵机控制，PWM 系统时钟输出的通道
	}
	else
	{
		__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, BULLETROOM_COVER_CLOSE); // close
	}
}

/***********************************************************************
** 函 数 名： robot_motor_speed_control_test()
** 函数说明： 机器人速度环控制调试
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void robot_motor_speed_control_test(void) // 用于测试，这里云台不跟随底盘
{
	float vx, vy, vw;

	//
	vx = rc_ctrl.ch4 * 10.0f;
	vy = -1.0f * rc_ctrl.ch3 * 10.0f;
	vw = rc_ctrl.wheel * 5.0f;

	robot_motor_speed.chassis_speed[0] = -(vx + vy + vw); // 姿态解算的结果
	robot_motor_speed.chassis_speed[1] = vx - vy - vw;
	robot_motor_speed.chassis_speed[2] = vx + vy - vw;
	robot_motor_speed.chassis_speed[3] = -(vx - vy + vw);

	//
	robot_motor_speed.yaw_speed = rc_ctrl.ch1 * 0.2f; // 遥控器赋值，通过遥控器传回数据赋值
	robot_motor_speed.pitch_speed = rc_ctrl.ch2 * 0.2f;

	//
	HAL_GPIO_WritePin(LASER_GPIO_Port, LASER_Pin, GPIO_PIN_SET); // open laser
}
